package com.grt192.controller;

import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.core.controller.EventController;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.mechanism.CreateBase;
import com.grt192.mechanism.SPOTDriverStation;
import com.grt192.mechanism.spot.SLight;
import com.grt192.sensor.DemoJoystick;
import com.grt192.utils.Util;

/**
 * Arcade style drive of a CreateBase given a single DemoJoystick
 * @author Andrew Chen <andrewtheannihilator@gmail.com>
 */
public class CreateTeleop extends EventController implements SensorChangeListener {

    private static final double DEADZONE = .2;
    private static final double G = 1.5;
    private static final int threadLight = 6;
    private static final int xLight = 4;
    private static final int yLight = 5;
    private final CreateBase base;
    private final SPOTDriverStation ds;
    private double x, y;

    /**
     * 
     * @param base base to drive
     * @param ds driver station to read Joystick from
     */
    public CreateTeleop(CreateBase base, SPOTDriverStation ds) {
        this.base = base;
        this.ds = ds;
        SLight.get(threadLight).rawColor(GRTDemoLED.Color.ORANGE);
    }

    public void run() {
        SLight.get(threadLight).rawColor(GRTDemoLED.Color.GREEN);
        running = true;
        while (running) {
            SLight.get(threadLight).blinkToBlack();
            drive();//act
            Util.sleep(50);
        }
    }

    /** 
     * Converts gravity vectors on SPOT axes to arcade style drive
     */
    public void drive() {
        double vX = convertControl(getPercent(x));
        double vY = convertControl(getPercent(y));
        log("Drive: " + (vY - vX) + "\t " + vY + vX);
        base.percentDrive(vY - vX, vY + vX);
    }

    public void startListening() {
        ds.getJoystick().addSensorChangeListener(this);
    }

    public void stopListening() {
        ds.getJoystick().addSensorChangeListener(this);
    }

    /**
     * Transforms a percentage control for more exotic controls schemes(ie. square)
     * @param percent linear percent read from joystick [-1.0... 1.0]
     * @return transformed percent [-1.0 ... 1.0]
     */
    private static double convertControl(double percent) {
        double mult = percent <= 0 ? -1.0 : 1.0;
        return (percent * percent) * mult;//squared
    }

    /**
     * Converts gravity to a percent in the style of normal joysticks
     * @param gravity gravity vector [-1.5...1.5]
     * @return linear percent  [-1.0... 1.0]
     */
    private static double getPercent(double gravity) {
        return gravity / G;
    }

    public void sensorStateChanged(SensorEvent e, String key) {
        if (key.equals(DemoJoystick.X)) {
            double x = e.getData(key);
            this.x = Math.abs(x) > DEADZONE ? x : 0;
        } else if (key.equals(DemoJoystick.Y)) {
            double y = e.getData(key);
            this.y = Math.abs(y) > DEADZONE ? y : 0;
        }
    }
}
